import casadi as ca

def CreateIntegrator(system, dt_val):
    """
    创建基于RK4方法的积分器
    
    Args:
        system: IrisEOM系统对象
        dt_val: 时间步长
        
    Returns:
        integrator: CasADi积分器函数
    """
    # 获取系统函数
    system_dynamics = system.iris_eom
    
    # 定义符号变量
    y = system_dynamics.sx_in('state')
    u = system_dynamics.sx_in('input')
    dt = ca.SX.sym('dt')
    
    # RK4方法实现
    k1 = system_dynamics(y, u)
    k2 = system_dynamics(y + dt/2 * k1, u)
    k3 = system_dynamics(y + dt/2 * k2, u)
    k4 = system_dynamics(y + dt * k3, u)
    y_next = y + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
    
    # 创建积分器函数
    integrator = ca.Function(
        'integrator',
        [y, u, dt],
        [y_next],
        ['state', 'control', 'dt'],
        ['next_state']
    )
    
    return integrator